/********** Library Includes **************************************************/

#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <sys/time.h>


/********** Local Includes ****************************************************/

#include "wacawsource.h"
#include "objectdetector.h"
#include "approxDecomp.h"
#include "common.h"
#include "celldecompastar.h"
#include "simpledrivecontroller.h"
#include "rovio.h"
#include "astarheuristics.h"

/********** Macros and Constants **********************************************/

#define DEBUG true
#define ROVIO_HOST "192.168.10.18"
#define TIME 500

/********** Namespaces ********************************************************/

using namespace cv;
using namespace std;

/********** Local Functions ***************************************************/

Point robot(231, 241), fruit(662, 241);
/**
 * A static helper function that returns the time with millisecond precision.
 */
long getTime();


void on_mouse(int event, int x, int y, int flags, void *)
{
	static int count = 0;
	
	switch(event) {
		case CV_EVENT_LBUTTONDOWN:
			if (count == 0) {
				robot = Point(x, y);
				cout << "Got robot position: (" << x << ", " << y << ")" << endl;
			} else if (count == 1) {
				fruit = Point(x, y);
				cout << "Got fruit position: (" << x << ", " << y << ")" << endl;
			}
			
			count++;
			break;
	}
}

int manhatten(Point a, Point b) {
	return abs(a.x - b.x) + abs(a.y - b.y);
}

void showPlan(const Mat &image, const vector<Point> plan, int blockSize, int next = -1) {
	Mat temp = image.clone();
	int count = 0;
	
	static bool first = true;
	int total = 0;
	
	for (vector<Point>::const_iterator it = plan.begin(); it != plan.end(); ++it) {
		Point p(it->x * blockSize + blockSize / 2,
			it->y * blockSize + blockSize / 2);
			
		if (count == next) {
			circle(temp, p, 7, CV_RGB(0, 0, 255), CV_FILLED);
		} else {
			circle(temp, p, 5, CV_RGB(255, 0, 0), CV_FILLED);
		}
		
		count++;
		vector<Point>::const_iterator next = it + 1;
		
		if (next != plan.end()) {
			Point a(it->x * blockSize + blockSize / 2,
				it->y * blockSize + blockSize / 2);
			Point b(next->x * blockSize + blockSize / 2,
				next->y * blockSize + blockSize / 2);
				
			line(temp, a, b, CV_RGB(255, 0, 0));
			total += manhatten(a, b);
		}
	}
	
	if (first) {
		cout << "Total path length: " << total << endl;
		first = false;
	}
	
	imshow("Plan", temp);
	cvMoveWindow("Plan", 0, 900 - temp.size().height);
}

int main(int, char **) {
    long startTime; 
	cout << "Press 'n' to step forward to the next frame." << endl
		<< "Press 'q' to close." << endl
		<< "In debug view:" << endl
		<< "\tRed is obstacle" << endl
		<< "\tGreen is fruit" << endl
		<< "\tBlue is robot" << endl;
	
	WacawSource *source = new WacawSource();
	ObjectDetector detector(source);
    
	detector.saveBackground();
    
	imshow("Processed", detector.processed());
	imshow("Raw", source->last());
	
	cvMoveWindow("Processed", 0, 0);
	cvMoveWindow("Raw", source->last().size().width, 0);
	
	cout << "Place obstacles and press 'n'" << endl;
	
	while (cvWaitKey(300) != 'n');
	
	detector.saveObstacles();
	
	imshow("Processed", detector.processed());
	imshow("Raw", source->last());
	
	cout << "Place and robot and fruit and press 'n'" << endl;
	
	while (cvWaitKey(300) != 'n');

	imshow("Live", source->next());
	cvSetMouseCallback("Live", on_mouse, 0);
	
	cout << "Click near the robot and fruit (in that order), then press 'c'"
		<< endl;
	
	int key;
	
	while (true) {
		key = waitKey(300);
		
		if (key == 'q') {
			return 0;
		} else if (key == 'c') {
            startTime = getTime();
			break;
		}
	}
	
	cvDestroyWindow("Live");
	
	if (DEBUG) {
		cvNamedWindow("Robot Blobs");
		cvMoveWindow("Robot Blobs", source->last().size().width,
			900 - source->last().size().height);
	}
	
	detector.saveRobotAndFruit(robot, fruit, DEBUG);
    
    Mat open = detector.getOpen();
    ApproxDecomp decomp(source, open);
    
    Mat grid = decomp.cellDecomp();
	CellDecompAStar astar(grid, new ManhattenDistanceHeuristic());
	
	imshow("Processed", detector.processed());
	imshow("Raw", source->last());
	
	Point robot = detector.robot().point;
	Point fruit = detector.fruit();
	
	Point start(robot.x / decomp.blockSize(), robot.y / decomp.blockSize());
	Point goal(fruit.x / decomp.blockSize(), fruit.y / decomp.blockSize());
	
	vector<Point> plan = astar.plan(start, goal);
    std::cout << "Planning time: " << getTime() - startTime << std::endl;
	showPlan(source->last(), plan, decomp.blockSize());
    startTime = getTime();
	DUMP(plan);
	
	DriveController *controller = new SimpleDriveController();
	Rovio rovio(ROVIO_HOST);
	size_t step = 0; // The step in the plan
	
	for (int i = 0; i < 2; i++) {
        startTime = getTime();
		while (step < plan.size()) {

			int key = cvWaitKey(300);
			
			if (key == 'q') {
				break;
			}
			
			RobotOrientation current = detector.findRobot(DEBUG);
			Command command;
			
			do {
				command = controller->next(current, plan[step] * decomp.blockSize());
				
				if (command.direction == DIR_STOP) {
					step++;
					cout << "Moving on to next step" << endl;
				}
			} while (command.direction == DIR_STOP);
			
			showPlan(source->last(), plan, decomp.blockSize(), step);
			
			imshow("Processed", detector.processed());
			imshow("Raw", source->last());
			
			waitKey(1);
			rovio.timeDrive(command.direction, command.time, command.speed);
		}
		
		step = 0;
        std::cout << "Time To the Fruit: " << getTime() - startTime << std::endl;

		reverse(plan.begin(), plan.end());
	}
}


long getTime() {
    struct timeval time;
    gettimeofday(&time, NULL);
    return ((time.tv_sec) * 1000 + time.tv_usec/1000.0) + 0.5;
}

